/*
 * logic.c
 *
 *  Created on: 2012.12.30.
 *      Author: Sunshineprincess
 */

#include "stm32f4xx.h"
#include "drive_control.h"
#include "steering_control.h"
#include "steering.h"
#include "Inertial.h"
#include "LineTracker.h"
#include "Rotary.h"
#include "logic.h"
#include "SharpSensors.h"

uint32_t state = 0;
uint16_t time = 0;
uint32_t wall_suspect_left = 0;
uint32_t wall_suspect_right = 0;
uint32_t wall_suspect_front = 0;
uint32_t prev_front = 100;
uint32_t line_suspect = 0;
uint32_t wall;
float prev_wall = 0;
float first_wall = 0;
uint32_t substate = 0;
uint32_t increment = 0;
uint8_t cross_line_suspect;
uint32_t direction;
uint32_t obstacle_done = 0;
uint32_t parking_done = 0;
uint32_t zero_speed_suspected = 0;
uint32_t wall_disappeared = 0;
uint32_t pre_finish = 0;
float line;

void Logic(void)
{
	switch(state)
	{
//----------------------------------------------------------------------------
		case STATE_START:
			time++;
			if(time == 100)
			{
				offset.gyrox = inertia.rotx / 100;
				offset.gyroy = inertia.roty / 100;
				offset.gyroz = inertia.rotz / 100;

				offset.accx = inertia.accx;
				offset.accy = inertia.accy;
				offset.accz = inertia.accz - 1;

				inertia.rotx = 0;
				inertia.roty = 0;
				inertia.rotz = 0;

				time = 0;
				state = STATE_INIT;
			}
			break;
//----------------------------------------------------------------------------
		case STATE_INIT:
			time = 0;
			wall_suspect_left = 0;
			wall_suspect_right = 0;
			wall_suspect_front = 0;
			zero_speed_suspected = 0;
			prev_front = 100;
			line_suspect = 0;
			substate = 0;
			increment = 0;
			cross_line_suspect = 0;
			wall_disappeared = 0;
			pre_finish = 0;
			direction = 0;
			state = STATE_LINETRACK_NORMAL;

		case STATE_LINETRACK_NORMAL:
			if(LineArray[0].valid)
				Steering_Control(11.5f, LineArray[0].linepos,90,0,500);
			Drive_Control(SPEED_NORMAL,speed);

			/* Finish line detection */
			if(LineArray[0].linewidth > 20)
			{
				if(cross_line_suspect < CROSS_LINE_TIMEOUT) cross_line_suspect++;
				else
				{
					pre_finish = 1;
					cross_line_suspect = 5;
				}

			} else if(pre_finish == 1)
			{
				cross_line_suspect--;
				if(cross_line_suspect == 0)
				{
					state = STATE_FINISH;
				}
				break;
			}
			else if(cross_line_suspect > 0)cross_line_suspect--;

			/* Wall detection */
			wall = (SharpSensorDistances[LEFT_SENS] < WALL_DETECT_LIMIT)? WALL_LEFT : 0;
			if(SharpSensorDistances[RIGHT_SENS] < WALL_DETECT_LIMIT) wall |= WALL_RIGHT;

			if((wall & WALL_LEFT) == WALL_LEFT)
			{
				wall_suspect_left++;
				if(wall == WALL_BOTH)
				{
					wall_suspect_right++;
					if((wall_suspect_right > WALL_DETECT_TIMEOUT) && (wall_suspect_left > WALL_DETECT_TIMEOUT) && (!parking_done))
					{
							state = STATE_PARKING;
							wall_suspect_right = WALL_DETECT_TIMEOUT;
							wall_suspect_left = WALL_DETECT_TIMEOUT;
					}
				}
				else
				{
					if (wall_suspect_right > 0)wall_suspect_right--;
					if ((wall_suspect_left > WALL_DETECT_TIMEOUT) && (wall_suspect_right < 2))
					{
						wall_suspect_left = WALL_DETECT_TIMEOUT;
						state =  STATE_CHANGE_LANE | STATE_NO_LINE_WALL;
					}
				}
			}
			else
			{
				if (wall_suspect_left > 0)wall_suspect_left--;
				if(wall == WALL_RIGHT)
				{
					wall_suspect_right++;
					if ((wall_suspect_right > WALL_DETECT_TIMEOUT)&& (wall_suspect_left < 2))
					{
						wall_suspect_right = WALL_DETECT_TIMEOUT;
						state =  STATE_CHANGE_LANE | STATE_NO_LINE_WALL;
					}
				}
				else
				{
					if (wall_suspect_right > 0) wall_suspect_right--;
				}
			}

			/* Front obstacle detection */
			if((SharpSensorDistances[FRONT_SENS] < 100) && (!obstacle_done))
			{
				if((prev_front - SharpSensorDistances[FRONT_SENS]) > 0)
				{
					wall_suspect_front++;
					if((wall_suspect_front > WALL_DETECT_TIMEOUT) && (SharpSensorDistances[FRONT_SENS] < 75))
					{
						state = STATE_OBSTACLE;
						//line = 11.5 - LineArray[0].linepos;
					}
					prev_front = SharpSensorDistances[FRONT_SENS];
				}
			}
			else if(wall_suspect_front > 0) wall_suspect_front--;
			break;
//----------------------------------------------------------------------------
		case STATE_CHANGE_LANE | STATE_NO_LINE_WALL:
			Drive_Control(SPEED_NORMAL,speed);
			if(LineArray[0].valid)
				Steering_Control(11.5f, LineArray[0].linepos,90,0,0);
			if(wall == WALL_LEFT)
			{
				if((SharpSensorDistances[LEFT_SENS] > WALL_DETECT_LIMIT) && LineArray[0].valid)
				{
					if (wall_suspect_left > 0) wall_suspect_left--;
					else  state = STATE_CHANGE_LANE;
				}
				else if(!LineArray[0].valid)
				{
					line_suspect++;
					if(line_suspect > LINE_THRESHOLD) /* Previously it was 4*/
					{
						state = STATE_NO_LINE_WALL;
					}
				}
				else
				{
					if(line_suspect > 0) line_suspect--;
					if(wall_suspect_left < WALL_DETECT_TIMEOUT)wall_suspect_left++;
				}
			}
			else
			{
				if((SharpSensorDistances[RIGHT_SENS] > WALL_DETECT_LIMIT) && LineArray[0].valid)
				{
					if (wall_suspect_right > 0)wall_suspect_right--;
					else  state = STATE_CHANGE_LANE;
				}
				else if(!LineArray[0].valid)
				{
					line_suspect++;
					if(line_suspect > LINE_THRESHOLD)
					{
						state = STATE_NO_LINE_WALL;
					}
				}
				else
				{
					if(line_suspect > 0) line_suspect--;
					if(wall_suspect_right < WALL_DETECT_TIMEOUT)wall_suspect_right++;
				}
			}
			break;
//----------------------------------------------------------------------------
		case STATE_CHANGE_LANE:
			Drive_Control(SPEED_CHANGE_LANE,speed);
			time++;
			increment += speed;

			if(increment > METER * 0.65)
			{
				if(wall == WALL_LEFT)
				{
					SetSteeringPWM(-300);
					if(LineArray[0].valid == 1 && LineArray[0].linepos > 13) state = STATE_INIT;
				}
				else
				{
					SetSteeringPWM(300);
					if(LineArray[0].valid == 1 && LineArray[0].linepos < 10) state = STATE_INIT;
				}
			}
			else
			{
				if(increment > METER * 0.6)			/* TODO: Distance */
				{
					SetSteeringPWM(0);
				}
				else
				{
					if(wall == WALL_LEFT) SetSteeringPWM(1050);
				    else SetSteeringPWM(-1050);
				}
			}
			break;
//----------------------------------------------------------------------------
		case STATE_NO_LINE_WALL:
			Drive_Control(SPEED_WALL_TRACKING,speed);
 			if(substate == 0)
 			{
				if(wall == WALL_LEFT)
				{
					Steering_Control(13,SharpSensorDistances[LEFT_SENS],130,0,0);
				} else
				{
					Steering_Control(-13,-SharpSensorDistances[RIGHT_SENS],130,0,0);
				}
				/* Waiting for line */
				if(LineArray[0].valid == 1)
				{
					if(line_suspect < LINE_THRESHOLD)
					{
						line_suspect++;
					} else
					{
						substate = 1;
					}
				}
				else if(line_suspect > 0)
				{
					line_suspect--;
				}
 			}
 			/* Substate: line found, wall still exists*/
 			else
 			{
 				if(LineArray[0].valid)	Steering_Control(11.5f, LineArray[0].linepos,90,0,0);
 				if(wall == WALL_LEFT)
 				{
					if((SharpSensorDistances[LEFT_SENS] > WALL_DETECT_LIMIT))
					{
						if (wall_suspect_left > 0) wall_suspect_left--;
						else state = STATE_INIT;
					}
					else
					{
						if(wall_suspect_left < WALL_DETECT_TIMEOUT) wall_suspect_left++;
					}
 				} else
 				{
					if((SharpSensorDistances[RIGHT_SENS] > WALL_DETECT_LIMIT))
					{
						if (wall_suspect_right > 0) wall_suspect_right--;
						else state = STATE_INIT;
					}
					else
					{
						if(wall_suspect_right < WALL_DETECT_TIMEOUT) wall_suspect_right++;
					}
 				}
 			}
			break;
//----------------------------------------------------------------------------
		case STATE_OBSTACLE:

			increment += speed;
			switch(substate)
			{
			case 0:
				Drive_Control(SPEED_OBSTACLE, speed);
				if(SharpSensorDistances[LEFT_SENS] < 30)
				{
					wall_suspect_left++;
					if(wall_suspect_left > WALL_DETECT_TIMEOUT - 2)
					{
						substate = 1;
						prev_wall = SharpSensorDistances[LEFT_SENS];
						first_wall = prev_wall;
						if(10.0 - first_wall > 4.0)
						{
							first_wall += 5.0;
						} else
						{
							first_wall = 10.0;
						}
						direction = 0;
						wall_suspect_left = WALL_DETECT_TIMEOUT - 3;
					}
				} else
				{
					if(wall_suspect_left > 0) wall_suspect_left--;
				}
				if(increment < METER * 0.51) SetSteeringPWM(1050);
				else if(increment < METER * 1.12) SetSteeringPWM(-1050);
				else{
					SetSteeringPWM(0);
/*
					if(SharpSensorDistances[LEFT_SENS] < 30)
					{
						wall_suspect_left++;
						if(wall_suspect_left > WALL_DETECT_TIMEOUT - 2)
						{
							substate = 1;
							prev_wall = SharpSensorDistances[LEFT_SENS];
							first_wall = prev_wall;
							if(12.0 - first_wall > 5.0)
							{
								first_wall += 5.0;
							} else
							{
								first_wall = 12.0;
							}
							direction = 0;
							wall_suspect_left = WALL_DETECT_TIMEOUT - 3;
						}
					} else
					{
						if(wall_suspect_left > 0) wall_suspect_left--;
					}
*/

				}
				break;
			case 1:
				Drive_Control(SPEED_OBSTACLE , speed);
				if(SharpSensorDistances[LEFT_SENS] < 30)
				{
					if(wall_suspect_left < WALL_DETECT_TIMEOUT - 3)wall_suspect_left++;
					if(ABS(SharpSensorDistances[LEFT_SENS] - prev_wall) > 2) direction++;
					prev_wall = SharpSensorDistances[LEFT_SENS];
					if(SharpSensorDistances[LEFT_SENS] < 25.0) Steering_Control(first_wall,SharpSensorDistances[LEFT_SENS],130,0,0);
				}
				else
				{
					if(wall_suspect_left > 0) wall_suspect_left--;
					else if(increment > METER * 1.65)
					{
						substate = 2;
					}
				}
				break;
			case 2:
				Drive_Control(SPEED_OBSTACLE, speed);
				if(direction > 3) SetSteeringPWM(950);
				else SetSteeringPWM(-950);
				if(LineArray[0].valid)
				{
					line_suspect++;
					if(line_suspect > LINE_THRESHOLD)
					{
						if(((LineArray[0].linepos <= 11.5) && (direction > 3)) || ((LineArray[0].linepos >= 11.5) && (direction < 4)))
						{
							obstacle_done = 1;
							state = STATE_INIT;
						}

					}
				}
				else if(line_suspect > 0)line_suspect--;
				break;
			}
			break;
//--------------------------------FINISH-----------------------------------
		case STATE_FINISH:
		switch(substate)
		{
		case 0:
			Drive_Control(SPEED_FINISH,speed);
			if(LineArray[0].valid)
			{
				Steering_Control(11.5f, LineArray[0].linepos,90,0,0);
				if(LineArray[0].linewidth > 20)
				{
					if(cross_line_suspect < CROSS_LINE_TIMEOUT) cross_line_suspect++;
					else
					{
						substate = 1;
						break;
					}
				} else
				{
					if(cross_line_suspect > 0) cross_line_suspect--;
				}
			}
		break;
		case 1:
			Drive_Control(100,speed);
		break;
		}
		break;
//----------------------------------------------------------------------------
		case STATE_PARKING:

			wall = (SharpSensorDistances[LEFT_SENS] < WALL_DETECT_LIMIT)? WALL_LEFT : 0;
			if(SharpSensorDistances[RIGHT_SENS] < WALL_DETECT_LIMIT) wall |= WALL_RIGHT;
			increment += speed;
			switch(substate)
			{
			case 0:
				Drive_Control(SPEED_NORMAL,speed);
				if(LineArray[0].valid)	Steering_Control(11.5f, LineArray[0].linepos,90,0,1000);
				/* Left parking slot */
				if((wall & WALL_LEFT) == 0)
				{
					if(wall_suspect_left > 0) wall_suspect_left--;
					else
					{
						direction = WALL_LEFT;
						substate = 1;
					}
				}
				/* Right parking slot */
				else
				{
					if(wall_suspect_left < WALL_DETECT_TIMEOUT) wall_suspect_left++;
					if((wall & WALL_RIGHT) == 0)
					{
						if(wall_suspect_right > 0) wall_suspect_right--;
						else
						{
							direction = WALL_RIGHT;
							substate = 1;
						}
					}
					else if(wall_suspect_right < WALL_DETECT_TIMEOUT) wall_suspect_right++;
				}
				break;
			case 1:
				Drive_Control(SPEED_PARKING_ENTER,speed);
				if(LineArray[0].valid)	Steering_Control(11.5f, LineArray[0].linepos,90,0,1000);
				if(direction == WALL_LEFT)
				{
					if(wall & WALL_LEFT)
					{
						if(wall_suspect_left < WALL_DETECT_TIMEOUT - 3) wall_suspect_left++;
						else
						{
							substate = 2;
							increment = 0;
						}
					}
					else if(wall_suspect_left > 0) wall_suspect_left--;
				}
				else
				{
					if(wall & WALL_RIGHT)
					{
						if(wall_suspect_right < WALL_DETECT_TIMEOUT - 3) wall_suspect_right++;
						else
						{
							substate = 2;
							increment = 0;
						}
					}
					else if(wall_suspect_right > 0) wall_suspect_right--;
				}
				break;
			case 2:
				if(increment < METER * 0.30) //0.30
				{
					Drive_Control(SPEED_PARKING_ENTER,speed);
				} else
				{
					Drive_Control(0,speed);
					if(LineArray[0].valid)	Steering_Control(11.5f, LineArray[0].linepos,90,0,1000);
					if(speed == 0) zero_speed_suspected++;
					if(zero_speed_suspected > ZERO_DETECT_TIMEOUT)
					{
						zero_speed_suspected = 0;
						if(direction == WALL_RIGHT)
						{
							SetSteeringPWM(1050);
							increment = 0;
							substate = 3;
						} else
						{
							SetSteeringPWM(-1050);
							increment = 0;
							substate = 3;
						}
					}
				}
			break;
			case 3:
				/* LEFT PARKING */
				if(direction == WALL_LEFT)
				{
					if(increment <= METER * 0.75)
					{
						Drive_Control(-SPEED_PARKING_BACKWARD,-speed);
					}
					else if(increment <  1.30 * METER)
					{
						Drive_Control(-SPEED_PARKING_BACKWARD,-speed);
						SetSteeringPWM(1050);
					}
					else
					{
						Drive_Control(0,-speed);
						if(speed == 0)
						{
							SetSteeringPWM(0);
							zero_speed_suspected++;
						}
						if(zero_speed_suspected > ZERO_DETECT_TIMEOUT)
						{
							SetSteeringPWM(900);
							substate = 4;
							increment = 0;
						}
					}
				}
				/* RIGHT PARKING */
				else
				{
					if(increment <= METER * 0.58)
					{
						Drive_Control(-SPEED_PARKING_BACKWARD,-speed);
					}
					else if(increment <  1.28 * METER  )
					{
						Drive_Control(-SPEED_PARKING_BACKWARD,-speed);
						SetSteeringPWM(-1050);
					}
					else
					{
						Drive_Control(0,-speed);
						if(speed == 0)
						{
							SetSteeringPWM(0);
							zero_speed_suspected++;
						}
						if(zero_speed_suspected > ZERO_DETECT_TIMEOUT)
						{
							SetSteeringPWM(-1050);
							substate = 4;
							increment = 0;
						}
					}
				}
			break;
			case 4:
				Drive_Control(SPEED_PARKING_OUT,speed);
				if(direction == WALL_LEFT)
				{
					if(increment > METER * 0.39)
					{
						SetSteeringPWM(-500);
					}
				} else
				{
					if(increment > METER * 0.55)
					{
						SetSteeringPWM(500);
					}
				}
				if(LineArray[0].valid)
				{
					line_suspect++;
					if(line_suspect > LINE_THRESHOLD)
					{
						if(((direction == WALL_LEFT) && (LineArray[0].linepos > 13)) || ((direction == WALL_RIGHT) && (LineArray[0].linepos < 10)))
						{
							parking_done = 1;
							substate = 5;
						}
					}
				}
				else
				{
					if(line_suspect > 0)line_suspect--;
				}

			break;
			case 5:
				if(LineArray[0].valid)	Steering_Control(11.5f, LineArray[0].linepos,90,0,0);
				Drive_Control(SPEED_PARKING_EXIT,speed);
				if(wall == 0)
				{
					wall_disappeared++;
					if(wall_disappeared > 5) state = STATE_INIT;

				} else if(wall_disappeared > 0) wall_disappeared--;
			break;
			}
			break;
		default:
			Drive_Control(0 ,speed);
			break;
	}
}
